#include "path_smoother.h"

namespace PathSmoother{

// 五点梯度下降法
// 梯度下降法平滑路径
// https://blog.csdn.net/weixin_43879302/article/details/116092810
// 梯度下降法实现路径平滑
// https://zhuanlan.zhihu.com/p/118666410
void PathSmootherFivePoint(vector<Pose2D> src_points, Map2D* map_2d, vector<Pose2D>& out_points){
    if(src_points.size() < 5){
        out_points = src_points;
        return;
    }
    int N=5;
    vector<Pose2D> smooth_points;
    // P = -1/6 * (P-2 - 4P-1 + 6P - 4P1 + P2)
    float a = -1.0 / 6.0;
    for(int n=0; n<N; n++){     //优化N次
        smooth_points.clear();
        smooth_points.resize(src_points.size());
        for(int i=0; i<src_points.size(); ++i){
            Pose2D curr_point = src_points[i];  
            smooth_points[i] = curr_point;
            if(i < 2 || i > src_points.size()-3){
                continue;
            }
            else{
                Pose2D prev_2_point = smooth_points[i-2];
                Pose2D prev_1_point = smooth_points[i-1];
                Pose2D back_1_point = src_points[i+1];
                Pose2D back_2_point = src_points[i+2];
                Pose2D smooth_point = curr_point + a * (prev_2_point 
                    - 4.0 * prev_1_point 
                    + 6.0 * curr_point 
                    - 4.0 * back_1_point
                    + back_2_point);
                // cout << "i: " << i << " " << curr_point << " " << smooth_point << endl;
                if(map_2d){
                    Pixel2D pixel_smooth = map_2d->Pose2DToPixel2D(smooth_point);
                    if(map_2d->ValueIsValid(pixel_smooth)){
                        smooth_points[i] = smooth_point;
                    }
                }
            }
        }

        // 重新赋值
        src_points = smooth_points;
    }

    out_points.clear();
    out_points.swap(smooth_points);
}

};